| Note: This tutorial assumes you have completed previous tutorial siemens_cp1616/Tutorials/CP1616 in IO Device mode - Configuration. |
 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
CP1616 io IO Device mode - User node
Description: This tutorial teaches you how to write a simple ROS node for interfacing PROFINETKeywords: SIEMENS, PROFINET, CP1616, IO Device, User node
Tutorial Level: INTERMEDIATE
Next Tutorial: siemens_cp1616/Tutorials/CP1616 in IO Device mode - Runtime
This tutorial teaches you how to publish and subscribe to topics generated by PROFINET IO Device wrapper node. Following code is intended for use with hardware configuration described in previous tutorial. Complete IO Device tutorial package is available on github in siemens_tutorials repository.
The code
1 #include <ros/ros.h>
2 #include <std_msgs/MultiArrayDimension.h>
3 #include <std_msgs/UInt8MultiArray.h>
4
5 //declare callback functions
6 void subByteCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
7 void subRealCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
8 void subByteArrayCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
9 void subBidirectByteCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
10
11 int main(int argc, char *argv[])
12 {
13 ros::init(argc, argv, "cp1616_io_device_tutorial_node");
14 ros::NodeHandle nh;
15
16 //Subscribe to topics defined in yaml config file
17 ros::Subscriber sub_byte = nh.subscribe("/plc_output_byte_topic",
18 1, &subByteCallback);
19 ros::Subscriber sub_real = nh.subscribe("/plc_output_real_topic",
20 1, &subRealCallback);
21 ros::Subscriber sub_byte_array = nh.subscribe("/plc_output_byte_array_topic",
22 1, &subByteArrayCallback);
23 ros::Subscriber sub_bidirect_byte = nh.subscribe("/plc_bidirect_output_topic",
24 1, &subBidirectByteCallback);
25
26 //Create publihsers for topics defined in yaml config file
27 ros::Publisher pub_byte = nh.advertise<std_msgs::UInt8MultiArray>
28 ("/plc_input_byte_topic", 1);
29 ros::Publisher pub_real = nh.advertise<std_msgs::UInt8MultiArray>
30 ("/plc_input_real_topic", 1);
31 ros::Publisher pub_byte_array = nh.advertise<std_msgs::UInt8MultiArray>
32 ("/plc_input_byte_array_topic", 1);
33 ros::Publisher pub_bidirect_byte = nh.advertise<std_msgs::UInt8MultiArray>
34 ("/plc_bidirect_input_topic", 1);
35
36 //Define and initialize output variables
37 unsigned char output_byte = 0;
38 float output_real = 0;
39 unsigned char output_byte_array[] = {0, 0, 0, 0, 0, 0, 0, 0};
40 unsigned char bidirect_byte = 0xff;
41
42 //std_msgs::MultiArray variables
43 std_msgs::MultiArrayDimension msg_dim;
44 std_msgs::UInt8MultiArray msg;
45
46 while(ros::ok())
47 {
48 //-------------------------------------
49 //Increment and publish PLC_input_byte
50 //-------------------------------------
51 msg_dim.label = "PLC_input_byte";
52 msg_dim.size = 1;
53 msg.layout.dim.clear();
54 msg.layout.dim.push_back(msg_dim);
55
56 output_byte++;
57 msg.data.clear();
58 msg.data.push_back(output_byte);
59 pub_byte.publish(msg);
60
61 //------------------------------------
62 //Increment and publish PLC_input_real
63 //------------------------------------
64 msg_dim.label = "PLC_input_real";
65 msg_dim.size = 4;
66 msg.layout.dim.clear();
67 msg.layout.dim.push_back(msg_dim);
68
69 msg.data.clear();
70
71 //Convert float to 4 bytes
72 output_real += 0.1;
73 unsigned char *p_byte;
74 unsigned int output_array[sizeof(float)];
75 p_byte = (unsigned char*)(&output_real);
76 for(size_t i = 0; i < sizeof(float); i++)
77 {
78 output_array[i] = (static_cast<unsigned int>(p_byte[i]));
79 msg.data.push_back(output_array[3-i]);
80 }
81 pub_real.publish(msg);
82
83 //-------------------------------------------
84 //Increment and publish PLC_input_byte_array
85 //-------------------------------------------
86 for(size_t i = 0; i < 8; i++)
87 output_byte_array[i] += 1;
88
89 msg_dim.label = "PLC_input_byte_array";
90 msg_dim.size = 8;
91 msg.layout.dim.clear();
92 msg.layout.dim.push_back(msg_dim);
93
94 msg.data.clear();
95 for(size_t i = 0; i < 8; i++)
96 msg.data.push_back(output_byte_array[i]);
97
98 pub_byte_array.publish(msg);
99
100 //-------------------------------------------
101 //Decrement and publish PLC_bidirect byte
102 //-------------------------------------------
103 bidirect_byte--;
104 msg_dim.label = "PLC_bidirect";
105 msg_dim.size = 1;
106 msg.layout.dim.clear();
107 msg.layout.dim.push_back(msg_dim);
108
109 msg.data.clear();
110 msg.data.push_back(bidirect_byte);
111 pub_bidirect_byte.publish(msg);
112
113 //-------------------------------------------
114 //Sleep and SpinOnce
115 //-------------------------------------------
116 ros::Duration(0.5).sleep();
117 ros::spinOnce();
118 }
119
120 return (EXIT_SUCCESS);
121 }
122 // plc_output_byte_topic message recieved
123 void subByteCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg)
124 {
125 unsigned int recieved_byte = msg->data[0];
126 }
127
128 // plc_output_real_topic message recieved
129 void subRealCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg)
130 {
131 unsigned int raw[4];
132
133 raw[0] = msg->data[0];
134 raw[1] = msg->data[1];
135 raw[2] = msg->data[2];
136 raw[3] = msg->data[3];
137
138 //RAW bytes to float conversion
139 int sign = (raw[0] >> 7) ? -1 : 1;
140 int8_t exponent = (raw[0] << 1) + (raw[1] >> 7) - 126;
141 uint32_t fraction_bits = ((raw[1] & 0x7F) << 16) + (raw[2] << 8) + raw[3];
142
143 float fraction = 0.5f;
144 for (uint8_t ii = 0; ii < 24; ++ii)
145 fraction += ldexpf((fraction_bits >> (23 - ii)) & 1, -(ii + 1));
146
147 float significand = sign * fraction;
148 float recieved_real = ldexpf(significand, exponent);
149 }
150
151 // plc_output_byte_array_topic message recieved
152 void subByteArrayCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg)
153 {
154 unsigned int recieved_byte_array[8];
155
156 for(unsigned int i = 0; i < 8; i++)
157 recieved_byte_array[i] = msg->data[i];
158 }
159
160 // plc_bidirect_output_topic message recieved
161 void subBidirectByteCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg)
162 {
163 unsigned int recieved_byte = msg->data[0];
164 }
The code explained
In addition to ros.h we also need to include std_msgs::UInt8MultiArray headers.
Declare callback functions for subscribers.
6 void subByteCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
7 void subRealCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
8 void subByteArrayCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
9 void subBidirectByteCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg);
Subscribe to input topics defined in yaml config file.
17 ros::Subscriber sub_byte = nh.subscribe("/plc_output_byte_topic",
18 1, &subByteCallback);
19 ros::Subscriber sub_real = nh.subscribe("/plc_output_real_topic",
20 1, &subRealCallback);
21 ros::Subscriber sub_byte_array = nh.subscribe("/plc_output_byte_array_topic",
22 1, &subByteArrayCallback);
23 ros::Subscriber sub_bidirect_byte = nh.subscribe("/plc_bidirect_output_topic",
24 1, &subBidirectByteCallback);
Create publishers for output topics defined in yaml config file.
27 ros::Publisher pub_byte = nh.advertise<std_msgs::UInt8MultiArray>
28 ("/plc_input_byte_topic", 1);
29 ros::Publisher pub_real = nh.advertise<std_msgs::UInt8MultiArray>
30 ("/plc_input_real_topic", 1);
31 ros::Publisher pub_byte_array = nh.advertise<std_msgs::UInt8MultiArray>
32 ("/plc_input_byte_array_topic", 1);
33 ros::Publisher pub_bidirect_byte = nh.advertise<std_msgs::UInt8MultiArray>
34 ("/plc_bidirect_input_topic", 1);
Create std_msgs::MultiArray variables
To send a message in MultiArray format to the wrapper, you need to fill the "label" and "size" in "msg.layout.dim" structure in accordance with yaml config file first.
Clear old data, update and publish to the output topic
In case when a float number (REAL in STEP7 format) needs to be sent, divide float to 4 bytes by following snippet of code
72 output_real += 0.1;
73 unsigned char *p_byte;
74 unsigned int output_array[sizeof(float)];
75 p_byte = (unsigned char*)(&output_real);
76 for(size_t i = 0; i < sizeof(float); i++)
77 {
78 output_array[i] = (static_cast<unsigned int>(p_byte[i]));
79 msg.data.push_back(output_array[3-i]);
80 }
81 pub_real.publish(msg);
Subscribing to MultiArray topic in case when only single byte is being transmitted:
When a REAL number is being transmitted, to convert 4 bytes back to float type, following code can be used:
129 void subRealCallback(const std_msgs::UInt8MultiArray::ConstPtr &msg)
130 {
131 unsigned int raw[4];
132
133 raw[0] = msg->data[0];
134 raw[1] = msg->data[1];
135 raw[2] = msg->data[2];
136 raw[3] = msg->data[3];
137
138 //RAW bytes to float conversion
139 int sign = (raw[0] >> 7) ? -1 : 1;
140 int8_t exponent = (raw[0] << 1) + (raw[1] >> 7) - 126;
141 uint32_t fraction_bits = ((raw[1] & 0x7F) << 16) + (raw[2] << 8) + raw[3];
142
143 float fraction = 0.5f;
144 for (uint8_t ii = 0; ii < 24; ++ii)
145 fraction += ldexpf((fraction_bits >> (23 - ii)) & 1, -(ii + 1));
146
147 float significand = sign * fraction;
148 float recieved_real = ldexpf(significand, exponent);
149 }
Notice that using bi-directional modules/slots/transfer_areas allows you to both publish and subscribe data within single slot:
If the code is ready and built, we can move to running the wrapper and checking the results:
Next tutorial: CP1616 - In IO Device mode - Runtime
